#include "Header.h"

int16_t Encoder_Count1;
int16_t Encoder_Count2;

int16_t Encoder_Speed1;
int16_t Encoder_Speed2;

/*编码器初始化*/
void Encoder_Init(void)
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    TIM_InternalClockConfig(TIM3);

    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Pin = E1A | E1B | E2A | E2B;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource0);
    GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource1);
    EXTI_InitTypeDef EXTI_InitStructure;
    EXTI_InitStructure.EXTI_Line = EXTI_Line0 | EXTI_Line1;
    EXTI_InitStructure.EXTI_LineCmd = ENABLE;
    EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
    EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
    EXTI_Init(&EXTI_InitStructure);

    TIM_TimeBaseInitTypeDef TIM_TimebaseInitStructure;
    TIM_TimebaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimebaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimebaseInitStructure.TIM_Period = 10000 - 1;
    TIM_TimebaseInitStructure.TIM_Prescaler = 144 - 1;
    TIM_TimebaseInitStructure.TIM_RepetitionCounter = 0;
    TIM_TimeBaseInit(TIM3, &TIM_TimebaseInitStructure);

    TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    NVIC_InitTypeDef NVIC_InitStructure;
    NVIC_InitStructure.NVIC_IRQChannel = EXTI0_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
    NVIC_Init(&NVIC_InitStructure);

    NVIC_InitStructure.NVIC_IRQChannel = EXTI1_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
    NVIC_Init(&NVIC_InitStructure);

    NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
    NVIC_Init(&NVIC_InitStructure);

    TIM_Cmd(TIM3, ENABLE);
}

/*返回左轮编码器目前的计数值*/
int16_t Encoder_GetCount(uint8_t n)
{
    if(n == 1) return Encoder_Count1;
    else if(n == 2) return Encoder_Count2;
    else return 0;
}

/*返回右轮编码器目前的计数值*/
int16_t Encoder_GetSpeed(uint8_t n)
{
    if(n == 1) return Encoder_Speed1;
    else if(n == 2) return Encoder_Speed2;
    else return 0;
}

/*通过外部中断实现编码器计数*/
void EXTI0_IRQHandler(void)
{
	if (EXTI_GetITStatus(EXTI_Line0) == SET)		
	{
		if (GPIO_ReadInputDataBit(GPIOB, E1A) == 0)
		{
			if (GPIO_ReadInputDataBit(GPIOB, E1B) == 1)	
			{
				Encoder_Count1 --;
			}
            else if(GPIO_ReadInputDataBit(GPIOB, E1B) == 0)
            {
                Encoder_Count1 ++;
            }
		}
		EXTI_ClearITPendingBit(EXTI_Line0);	
	}
}

void EXTI1_IRQHandler(void)
{
	if (EXTI_GetITStatus(EXTI_Line1) == SET)		
	{
		if (GPIO_ReadInputDataBit(GPIOB, E2A) == 0)
		{
			if (GPIO_ReadInputDataBit(GPIOB, E2B) == 1)
			{
				Encoder_Count2 ++;
			}
            else if(GPIO_ReadInputDataBit(GPIOB, E2B) == 0)
            {
                Encoder_Count2 --;
            }
		}
		EXTI_ClearITPendingBit(EXTI_Line1);
	}
}

void TIM3_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM3, TIM_IT_Update) == SET)
	{ 
        Encoder_Speed1 = Encoder_Count1;
        Encoder_Count1 = 0;
        Encoder_Speed2 = Encoder_Count2;
        Encoder_Count2 = 0;
        MPU_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);
		roll += (float)GX / 16.4 * 0.020;
		yaw += (float)GZ / 16.4 * 0.020;
		pitch += (float)GY / 16.4 * 0.020;
        TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
	}
}
